#!/usr/bin/env python3
import rospy
from drone_pkg.msg import QRCode
import cv2
import numpy as np
from pyzbar.pyzbar import decode
 


def Read_Decode_Pic(name, image, CamID):
    global pub
    for code in decode(image):
        data = code.data.decode('utf-8')
        print(data) 
        pts_poly= np.array(code.polygon, np.int32)  
        cv2.polylines(image, [pts_poly], True, (0, 255, 0), 5)  
        pts_rect = code.rect
        cv2.putText(image, data, (pts_rect[0],pts_rect[1]), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 0, 255), 2)
        msg = QRCode()
        msg.Data = str(data)
        msg.CamID = CamID
        msg.x = pts_rect[0]
        msg.y = pts_rect[1]
        msg.Width = pts_rect[2]
        msg.Height = pts_rect[3]
        pub.publish(msg)
    cv2.imshow(name, image)    
 
 
 
if __name__ == '__main__':            
    rospy.init_node("QRCode_detect", anonymous=True)
    pub = rospy.Publisher("/QRCode_detect", QRCode, queue_size=10)
    rate = rospy.Rate(100)
    cap0 = cv2.VideoCapture('/dev/came0')
    cap1 = cv2.VideoCapture('/dev/came1')
    while not rospy.is_shutdown():
        ret, frame0 = cap0.read()
        ret, frame1 = cap1.read()
        try:
            Read_Decode_Pic('0', frame0, 0)     
            Read_Decode_Pic('1', frame1, 1) 
        except:
            print("error")

        cv2.waitKey(1)
        rate.sleep()
    cv2.destroyAllWindows()       
    cv2.waitKey(0)


